
import { Vec3 } from "./Vec3";
import { MathUtil } from "./MathUtil";
import { Mat3 } from "./Mat3";
import { M } from "./M";

/**
 * Quaternion class.
 */
export class Quat {
	/**
	 * The number of instance creation.
	 */
	public static numCreations: number = 0;

	/**
	 * The x-value of the imaginary part of the quaternion.
	 */
	x: number;

	/**
	 * The y-value of the imaginary part of the quaternion.
	 */
	y: number;

	/**
	 * The z-value of the imaginary part of the quaternion.
	 */
	z: number;

	/**
	 * The real part of the quaternion.
	 */
	w: number;

	/**
	 * Creates a new quaternion. The quaternion is identity by default.
	 */
	constructor(x: number = 0, y: number = 0, z: number = 0, w: number = 1) {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
		Quat.numCreations++;
	}


	private initi(x: number, y: number, z: number, w: number): Quat {
		var tx: number = x;
		var ty: number = y;
		var tz: number = z;
		var tw: number = w;
		this.x = tx;
		this.y = ty;
		this.z = tz;
		this.w = tw;
		return this;
	}

	/**
	 * Sets the quaternion to identity quaternion and returns `this`.
	 */
	public identity(): Quat {
		return this.initi(0, 0, 0, 1);
	}

	/**
	 * Sets all values at once and returns `this`.
	 */
	public init(x: number, y: number, z: number, w: number): Quat {
		this.x = x;
		this.y = y;
		this.z = z;
		this.w = w;
		return this;
	}

	/**
	 * Returns `this` + `v`.
	 */
	public add(q: Quat): Quat {
		return new Quat(this.x + q.x, this.y + q.y, this.z + q.z, this.w + q.w);
	}

	public addScaleTo(q: Quat, scale: number, to: Quat) {
		to.init(
			this.x + q.x * scale, this.y + q.y * scale, this.z + q.z * scale, this.w + q.w * scale
		);
	}

	/**
	 * Returns `this` - `v`.
	 */
	public sub(q: Quat): Quat {
		return new Quat(this.x - q.x, this.y - q.y, this.z - q.z, this.w - q.w);
	}

	public subTo(q: Quat, to: Quat) {
		return to.init(this.x - q.x, this.y - q.y, this.z - q.z, this.w - q.w);
	}

	/**
	 * Returns `this` * `s`.
	 */
	public scale(s: number): Quat {
		return new Quat(this.x * s, this.y * s, this.z * s, this.w * s);
	}

	public scaleTo(s: number, to: Quat) {
		to.init(this.x * s, this.y * s, this.z * s, this.w * s);
	}

	/**
	 * Sets this quaternion to `this` + `v` and returns `this`.
	 */
	public addEq(q: Quat): Quat {
		return this.initi(this.x + q.x, this.y + q.y, this.z + q.z, this.w + q.w);
	}

	/**
	 * Sets this quaternion to `this` - `v` and returns `this`.
	 */
	public subEq(q: Quat): Quat {
		return this.initi(this.x - q.x, this.y - q.y, this.z - q.z, this.w - q.w);
	}

	/**
	 * Sets this quaternion to `this` * `s` and returns `this`.
	 */
	public scaleEq(s: number): Quat {
		return this.initi(this.x * s, this.y * s, this.z * s, this.w * s);
	}

	/**
	 * Returns the length of the quaternion.
	 */
	public length(): number {
		return MathUtil.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
	}

	/**
	 * Returns the squared length of the quaternion.
	 */
	public lengthSq(): number {
		return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
	}

	/**
	 * Returns the dot product of `this` and `q`.
	 */
	public dot(q: Quat): number {
		return this.x * q.x + this.y * q.y + this.z * q.z + this.w * q.w;
	}

	/**
	 * Returns the normalized quaternion.
	 *
	 * If the length is zero, zero quaterinon is returned.
	 */
	public normalized(): Quat {
		var invLen: number = this.length();
		if (invLen > 0) invLen = 1 / invLen;
		return new Quat(this.x * invLen, this.y * invLen, this.z * invLen, this.w * invLen);
	}

	/**
	 * Sets this quaternion to the normalized quaternion and returns `this`.
	 *
	 * If the length is zero, this quaternion is set to zero quaternion.
	 */
	public normalize(): Quat {
		var invLen: number = this.length();
		if (invLen > 0) invLen = 1 / invLen;
		return this.initi(this.x * invLen, this.y * invLen, this.z * invLen, this.w * invLen);
	}

	public normalizeTo(to: Quat) {
		var invLen: number = this.length();
		if (invLen > 0) invLen = 1 / invLen;
		return to.initi(this.x * invLen, this.y * invLen, this.z * invLen, this.w * invLen);
	}

	public negateTo(to: Quat) {
		to.init(-this.x, -this.y, -this.z, -this.w);
	}

	/**
	 * Sets this quaternion to the quaternion representing the shortest arc
	 * rotation from `v1` to `v2`, and return `this`.
	 */
	public setArc(v1: Vec3, v2: Vec3): Quat {
		var x1: number = v1.x;
		var y1: number = v1.y;
		var z1: number = v1.z;
		var x2: number = v2.x;
		var y2: number = v2.y;
		var z2: number = v2.z;
		var d: number = x1 * x2 + y1 * y2 + z1 * z2;

		this.w = MathUtil.sqrt((1 + d) * 0.5);

		if (this.w == 0) { // PI rotation, set a vector perpendicular to v1
			x2 = x1 * x1;
			y2 = y1 * y1;
			z2 = z1 * z1;
			switch (M.compare3min(x2, y2, z2)) {
				case -1:
					{
						// |x1| is the smallest, use x-axis
						d = 1 / MathUtil.sqrt(y2 + z2);
						this.x = 0;
						this.y = z1 * d;
						this.z = -y1 * d;
					}
					break;
				case 0:
					{
						// |y1| is the smallest, use y-axis
						d = 1 / MathUtil.sqrt(z2 + x2);
						this.y = 0;
						this.z = x1 * d;
						this.x = -z1 * d;
					}
					break;
				case 1:
					{
						// |z1| is the smallest, use z-axis
						d = 1 / MathUtil.sqrt(x2 + y2);
						this.z = 0;
						this.x = y1 * d;
						this.y = -x1 * d;
					}
					break;
			}
			return this;
		}

		// cos(theta/2) = sqrt((1 + cos(theta)) / 2)
		// sin(theta/2) / sin(theta) = sin(theta/2) / (2 * sin(theta/2) * cos(theta/2))
		//                           = 1 / (2 * cos(theta/2))
		d = 0.5 / this.w;

		// x^2 + y^2 + z^2 = sin(theta/2)
		var cx: number = y1 * z2 - z1 * y2;
		var cy: number = z1 * x2 - x1 * z2;
		var cz: number = x1 * y2 - y1 * x2;
		this.x = cx * d;
		this.y = cy * d;
		this.z = cz * d;
		return this;
	}

	/**
	 * Returns the spherical linear interpolation between two quaternions `this` and `q` with interpolation paraeter `t`.
	 * Both quaternions `this` and `q` must be normalized.
	 */
	public slerp(q: Quat, t: number): Quat {
		var qx: number;
		var qy: number;
		var qz: number;
		var qw: number;
		var d: number = this.dot(q);
		if (d < 0) {
			d = -d;
			qx = -q.x;
			qy = -q.y;
			qz = -q.z;
			qw = -q.w;
		} else {
			qx = q.x;
			qy = q.y;
			qz = q.z;
			qw = q.w;
		}
		if (d > 1 - 1e-6) {
			// two quaternions are too close, returns lerp instead
			return new Quat(this.x + (qx - this.x) * t, this.y + (qy - this.y) * t, this.z + (qz - this.z) * t, this.w + (qw - this.w) * t).normalize();
		}
		// target angle
		var theta: number = t * MathUtil.acos(d);

		// make q orthogonal to this
		qx -= this.x * d;
		qy -= this.y * d;
		qz -= this.z * d;
		qw -= this.w * d;
		var invLen: number = 1 / MathUtil.sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
		qx *= invLen;
		qy *= invLen;
		qz *= invLen;
		qw *= invLen;

		// mix them
		var sin: number = MathUtil.sin(theta);
		var cos: number = MathUtil.cos(theta);
		return new Quat(this.x * cos + qx * sin, this.y * cos + qy * sin, this.z * cos + qz * sin, this.w * cos + qw * sin);
	}

	/**
	 * Copies values from `q` and returns `this`.
	 */
	public copyFrom(q: Quat): Quat {
		this.x = q.x;
		this.y = q.y;
		this.z = q.z;
		this.w = q.w;
		return this;
	}

	/**
	 * Returns a clone of the quaternion.
	 */
	public clone(): Quat {
		return new Quat(this.x, this.y, this.z, this.w);
	}

	/**
	 * Sets this quaternion to the representation of the matrix `m`, and returns `this`.
	 *
	 * The matrix `m` must be a rotation matrix, that is, must be orthogonalized and have determinant 1.
	 */
	public fromMat3(m: Mat3): Quat {
		var e00: number = m.e00;
		var e11: number = m.e11;
		var e22: number = m.e22;
		var t: number = e00 + e11 + e22;
		var s: number;
		if (t > 0) {
			s = MathUtil.sqrt(t + 1);
			this.w = 0.5 * s;
			s = 0.5 / s;
			this.x = (m.e21 - m.e12) * s;
			this.y = (m.e02 - m.e20) * s;
			this.z = (m.e10 - m.e01) * s;
		} else {
			switch (M.compare3max(e00, e11, e22)) {
				case -1:
					{
						// e00 is the largest
						s = MathUtil.sqrt(e00 - e11 - e22 + 1);
						this.x = 0.5 * s;
						s = 0.5 / s;
						this.y = (m.e01 + m.e10) * s;
						this.z = (m.e02 + m.e20) * s;
						this.w = (m.e21 - m.e12) * s;
					}
					break;
				case 0:
					{
						// e11 is the largest
						s = MathUtil.sqrt(e11 - e22 - e00 + 1);
						this.y = 0.5 * s;
						s = 0.5 / s;
						this.x = (m.e01 + m.e10) * s;
						this.z = (m.e12 + m.e21) * s;
						this.w = (m.e02 - m.e20) * s;
					}
					break;
				case 1:
					{
						// e22 is the largest
						s = MathUtil.sqrt(e22 - e00 - e11 + 1);
						this.z = 0.5 * s;
						s = 0.5 / s;
						this.x = (m.e02 + m.e20) * s;
						this.y = (m.e12 + m.e21) * s;
						this.w = (m.e10 - m.e01) * s;
					}
					break;
			}
		}
		return this;
	}

	/**
	 * Returns a rotation matrix which represents this quaternion.
	 */
	public toMat3(): Mat3 {
		return new Mat3().fromQuat(this);
	}

	/**
	 * Returns the string representation of the quaternion.
	 */
	public toString(): String {
		return "Quat[" + this.x.toFixed(2) + " i,\n" +
			"    " + this.y.toFixed(2) + " j,\n" +
			"    " + this.z.toFixed(2) + " k,\n" +
			"    " + this.w.toFixed(2) + "]"
			;
	}

}
